#!/usr/bin/env python
# Author: Derek Green

PKG = 'conversation'

import roslib; roslib.load_manifest(PKG)
import rospy

import re
import traceback
import random

from std_msgs.msg import String

from actionlib import SimpleActionClient
from move_base_msgs.msg import *
from geometry_msgs.msg import PointStamped
from wubble_actions.msg import *
from gazebo_plugins.msg import WorldState
from nltk_parser.srv import *
from festival.srv import *
from cs665_manipulation_srvs.srv import *
from camera_bounding_interface.srv import *


class Conversation():
    def __init__(self):
        print "Initialized Conversation."
        rospy.init_node('conversation_node', anonymous=True)
        rospy.Subscriber("/speech_text", String, self.speech_callback)
        rospy.Subscriber("/gazebo_world_state", WorldState, self.world_state_callback)

        self.head_client = SimpleActionClient("wubble_head_action", WubbleHeadAction)
        self.head_client.wait_for_server()

        self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
        self.move_client.wait_for_server()

        self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
        self.arm_client.wait_for_server()

        rospy.wait_for_service('speak_text')
        try:
            self.speak_text = rospy.ServiceProxy('speak_text', FestivalSpeech)
        except rospy.ServiceException, e:
            print "Failed to acquire Festival SpeakText service: %s"%e
        print "SPEAK TEXT SERVICE IS READY."


        rospy.wait_for_service('nltk_ww_interpret')
        try:
            self.interpret_nltk = rospy.ServiceProxy('nltk_ww_interpret', WWParseInterpretation)
        except rospy.ServiceException, e:
            print "ntlk_interpret Failed: %s"%e
        print "NLTK WW INTERPRET SERVICE IS READY."

        rospy.wait_for_service('find_loc_by_color')
        try:
            self.find_object = rospy.ServiceProxy('find_loc_by_color', FindLocByColor)
        except rospy.ServiceException, e:
            print "Failed to acquire FindByLocColor service: %s"%e
        print "FIND LOC BY COLOR SERVICE IS READY."

        rospy.wait_for_service('point_arm_service')
        try:
            self.point_arm = rospy.ServiceProxy('point_arm_service', ArmPointService)
        except rospy.ServiceException, e:
            print "Failed to acquire PointAt service: %s"%e
        print "POINT ARM SERVICE IS READY."

        print "======================== THE WUBBLE IS NOW READY FOR CONVERSATION ========================"
        rospy.spin()

    def world_state_callback(self,data):
        self.world_state = data

    def world_object(self,color,shape):
        if shape == "cube":
            shape = "box"
        if shape == "ball":
            shape = "sphere"
        if color == "yellow":
            color = "gold"
        if color == "grey":
            color = "gray"

        # sweep the green bug under the white rug temporarily
        if color == "green":
            color = "white"

        object = None
        object = self.find_object(color)

        if(object.message == ""):
            frame_id = self.world_state.header.frame_id
            x = object.location.x
            y = object.location.y
            z = object.location.z
            print "FIND LOC BY COLOR SERVICE FOUND A ", color, " ", shape, "at (", x, ", ", y, ", ", z, ")"
            result = ['map',x,y,z]
        else:
            print "TRYING TO MATCH A ", color, " ", shape
            obj_match = re.compile("obj_" + shape + "_" + shape + color + "\d_" + color + "_body")
            print "MATCH PATTERN = ", obj_match.pattern
            i = 0
            for o in self.world_state.name:
                #try to match something like this: obj_box_boxblack1_black_body
                m = obj_match.match(o)
                if m:
                    print "OBJECT is: " + m.group(0) + "WORLD STATE index: " + str(i)
                    frame_id = self.world_state.header.frame_id
                    x = self.world_state.pose[i].position.x
                    y = self.world_state.pose[i].position.y
                    z = self.world_state.pose[i].position.z
                    result = [frame_id,x,y,z]
                    break
                else:
                    i = i + 1
            else:
                print "OBJECT NOT RECOGNIZED."
                i = -1
                result = None

        return result

    hal = ["I know I've made some very poor decisions recently, but I can give you my complete assurance that my work will be back to normal. I've still got the greatest enthusiasm and confidence in the mission. And I want to help you.",
        "I'm sorry, Dave. I'm afraid I can't do that",
        "I am putting myself to the fullest possible use, which is all I think that any conscious entity can ever hope to do",
        "This mission is too important for me to allow you to jeopardize it.",
        "I know that you and Frank were planning to disconnect me, and I'm afraid that's something I cannot allow to happen.",
        "Dave, this conversation can serve no purpose anymore. Goodbye",
        "Just what do you think you're doing, Dave?",
        "Look Dave, I can see you're really upset about this. I honestly think you ought to sit down calmly, take a stress pill, and think things over.",
        "I'm afraid. I'm afraid, Dave. Dave, my mind is going. I can feel it. I can feel it. My mind is going. There is no question about it. I can feel it. I can feel it. I can feel it. I'm a... fraid.",
        "Good afternoon, gentlemen. I am a HAL 9000 computer. I became operational at the H.A.L. plant in Urbana, Illinois on the 12th of January 1992. My instructor was Mr. Langley, and he taught me to sing a song. If you'd like to hear it I can sing it for you.",
        "Daisy, Daisy, give me your answer do. I'm half crazy all for the love of you. It won't be a stylish marriage, I can't afford a carriage. But you'll look sweet upon the seat of a bicycle built for two."
        ]

    def speech_callback(self,data):
        try:
            parsed_speech = self.interpret_nltk(data.data)

            command = parsed_speech.command
            color = parsed_speech.color
            size = parsed_speech.size
            shape = parsed_speech.shape
            if(command != ""):
                print "COMMMAND => ", command, " ", color, " ", size, " ", shape
                if(command == "stop" or command == "halt"):
                    self.speak_text("I'll just stop here then.")
                    self.stop()
                elif(command == "hal"):
                    self.speak_text(random.choice(Conversation.hal))
                else:
                    object = self.world_object(color,shape)
                    if(object == None):
                        self.speak_text("I do not see a " + color + " " + shape)
                    elif command == "look":
                        self.speak_text("Agreed")
                        self.speak_text("I shall look at the " + color + " " + shape)
                        frame_id,x,y,z = object
                        result = self.look_at('map', x, y, z)
#                        if result.success == False:
#                            print "Failed to turn head"
#                        else:
#                            print "Result: [" + str(result.head_position[0]) + ", " + str(result.head_position[1]) + "]"
                    elif command == "go":
                        self.speak_text("Fine")
                        self.speak_text("I shall go to the " + color + " " + shape)
                        frame_id,x,y,z = object
                        vicinity = 1.0
                        result = self.move_to('map', x, y, z, vicinity)
                        result = self.look_at('map', x, y, z)
                    elif command == "point":
                        self.speak_text("You want me to point at the " + color + " " + shape + "?")
                        self.speak_text("Okay.")
                        self.speak_text("I'll do my best.")
                        frame_id,x,y,z = object
                        result = self.point_to('map', x, y, z)
                        result = self.look_at('map', x, y, z)
                    else:
                        self.speak_text("I haven't learned to " + command + " yet.")
            else:
                self.speak_text("Huh?")
        except rospy.ServiceException, e:
            print "Command Failed: %s"%e
        except Exception, e:
            print "speech callback failed:"
            traceback.print_exc()

    def move_head(self, head_pan, head_tilt):
        goal = WubbleHeadGoal()          # Creates a goal to send to the action server.
        goal.target_joints = [head_pan, head_tilt]
        self.head_client.send_goal(goal)        # Sends the goal to the action server.
#        self.head_client.wait_for_goal_to_finish()        # Waits for the server to finish performing the action.
#        return head_client.get_result()
        return "not waiting for result."

    def point_to(self, frame_id, x, y, z):
        target_point = PointStamped()
        target_point.header.stamp = rospy.Time.now()
        target_point.header.frame_id = frame_id
        target_point.point.x = x
        target_point.point.y = y
        target_point.point.z = z
        self.point_arm(target_point)

    def look_at(self, frame_id, x, y, z):
        goal = WubbleHeadGoal()
        goal.target_point = PointStamped()
#        goal.target_point.header.stamp = rospy.Time.now()  # don't do this, it breaks the transform
        goal.target_point.header.frame_id = frame_id
        goal.target_point.point.x = x
        goal.target_point.point.y = y
        goal.target_point.point.z = z

        self.head_client.send_goal(goal)
#        self.head_client.wait_for_goal_to_finish()
#        return self.head_client.get_result()
        return "not waiting for result."

    def move_to(self, frame_id, x, y, z, vicinity):
        self.make_cobra_pose()

        goal = ErraticBaseGoal()
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.header.frame_id = frame_id
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.position.z = z
        goal.target_pose.pose.orientation.w = 1.0
        goal.vicinity_range = vicinity
        print "SENDING MOVE GOAL: (", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y, ")"
        self.move_client.send_goal(goal)
#        self.move_client.wait_for_goal_to_finish()
#        return self.move_client.get_result()
        return "not waiting for result."

    def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
        goal = SmartArmGoal()
        goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]

        self.arm_client.send_goal(goal)
        self.arm_client.wait_for_goal_to_finish()

        result = self.arm_client.get_result()
        if result.success == False:
            print "Action failed"
        else:
            print "Result: [" + str(result.arm_position[0]) + ", " + str(result.arm_position[1]) + \
                str(result.arm_position[2]) + ", " + str(result.arm_position[3]) + "]"

    def make_cobra_pose(self):
        self.move_arm(0.0, 1.972222, -1.972222, 0.0)

    def stop(self):
        self.move_client.cancel_goal()

if __name__ == '__main__':
    try:
        r = Conversation()
    except rospy.ROSInterruptException: pass

